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ABSTRACT 

Many  remotely  operated  robotic  manipulator  systems  are 
operated  in  rate  control  mode  to  achieve  a  commanded  position 
and  orientation  of  the  end-effector.  Performance  of  certain 
tasks,  such  as  applying  a  torque  to  a  screw,  would  be  more 
efficient  if  performed  in  unilateral  force  control  mode.  A  six 
axis  force-torque  model  was  developed  to  determine  the 
required  number  and  positioning  of  sensors  and  using  force 
sensing  resistors,  a  prototype  force-torque  transducer  was 
built  for  testing.  Using  a  force  error  signal,  individual 
manipulator  joint  angles  may  be  computed  in  an  algorithm  to 
achieve  force  replication  in  the  end-effector. 


in 


TABLE  OF  CONTENTS 

I.  INTRODUCTION   1 

II.  PRELIMINARY  WORK 4 

A.   FORCE  OVERRIDE  RATE  CONTROLLER   4 

1.  Objectives 4 

2 .  Background 4 

3.  Technical  Approach  5 

4.  Control  Analysis  8 

5.  Simulation  and  Testing 11 

III.  SENSOR  DEVELOPMENT   13 

A.   THEORY 13 

1.  Force  Sensing 13 

2.  Force  Sensing  Resistors   15 

3.  Transducer  Design   16 

a.  Preliminary  Design  16 

b.  Sensor  Placement  17 

4.  Development  of  Sensor  Equations   19 

5.  Sensor  Placement    21 

a.  Methodology 21 

b.  Transducer  Design  Optimization  29 

c.  Reduced  Order  Transducer  31 


IV 


DUDLEY  KNOX  LIBRARY 
MONTEREY  CA  93943-5101 

B.   PROTOTYPE  TRANSDUCER  DESIGN  34 

1 .  Mechanical  Design   34 

2.  Transducer  Testing  and  Calibration  37 

IV.  FORCE  CONTROL 39 

A.  THEORY 39 

1.  Robotic  Application  of  Transducer   39 

2.  Manipulator  Kinematics  39 

a.  Generalized  Coordinate  Transformations  .  39 

b.  Compound  Transformations  41 

c.  Denavit-Hartenberg  Transformations  ...  43 

d.  PUMA  560  Denavit-Hartenberg  Parameters  .  46 

e.  PUMA  560  Forward  Kinematics 48 

f.  Inverse  Manipulator  Kinematics  49 

3.  Jacobian  Development  51 

a.  Static  Forces  in  Manipulators   51 

b.  Manipulator  Jacobian  in  the  Force  Domain  53 

B.  IMPLEMENTATION 54 

1.  Force  Override  Rate  Controller  Algorithm  .  .  54 

V.  CONCLUSIONS 55 

A.  EXPERIMENTAL  AND  THEORETICAL  RESULTS   55 

B.  FURTHER  WORK 55 

APPENDIX  A 56 


v 


APPENDIX  B 61 


APF-ENDIX  C 64 


LIST  OF  REFERENCES 67 


INITIAL  DISTRIBUTION  LIST  69 


VI 


LIST  OF  TABLES 

TABLE  1.  ASSUMED  GAIN  CONSTANTS 12 

TABLE  2.  BIPLANAR  TRANSDUCER  COEFFICIENT  ARRAY   ...    23 

TABLE  3.  SIX  AXIS  TRANSDUCER  COEFFICIENT  ARRAY   ...    26 

TABLE  4.  PROTOTYPE  TRANSDUCER  WIRING  CODE 35 

TABLE  5.  OPERABLE  SENSOR  COEFFICIENT  ARRAY 37 

TABLE  6.  PUMA  560  LINK  AND  JOINT  PARAMETERS 47 

TABLE  7.  54  SENSOR  COEFFICIENT  ARRAY 56 

TABLE  8.  SENSOR  ELIMINATION  STRATEGY 58 

TABLE  9.  24  SENSOR  COEFFICIENT  ARRAY 59 

TABLE  10.  SENSOR  ELIMINATION  STRATEGY 60 


VII 


LIST  OF  FIGURES 

Figure  1.  PUMA  560  Manipulator  Washing  a  Window   ...  2 

Figure  2.  Wrist  Mounted  Force/Torque  Sensor     ....  5 

Figure  3.  Single  DOF  Hydraulic  Actuation  System   ...  6 

Figure  4 .  Force  Override  Rate  Control  Loop  8 

Figure  5.  Controller  Block  Diagram  9 

Figure  6.  Modified  Controller  Block  Diagram   11 

Figure  7.  Controller  Response  to  Step  Input   12 

Figure  8.  FSR  and  Characteristics 16 

Figure  9.  FSR-Based  Transducer  Conceptual  Design  ...  17 

Figure  10.  Planar  Joystick     20 

Figure  11.  Transducer  with  8  Sensors 22 

Figure  12.  Six  Axis  Transducer 25 

Figure  13.  54  Sensor  Transducer  27 

Figure  14.  Optimized  Full  Order  Transducer   30 

Figure  15.  Reduced  Order  Transducer  33 

Figure  16.  Prototype  Transducer  Mechanical  Design  ...  36 

Figure  17.  Robotic  Application  of  Transducer   40 

Figure  18.  Translated  and  Rotated  Link  Frame   42 

Figure  19.  Generic  Manipulator  Link  45 

Figure  20.  PUMA  Link  Frame  Assignments 47 

Figure  21.  Forces  on  a  Manipulator  Link 51 

Figure  22.  Force  Control  Algorithm   54 


vm 


ACKNOWLEDGMENT 

I  wish  to  express  my  appreciation  to  NASA  for  their 
sponsorship  and  funding  of  this  project,  to  the  Naval 
Postgraduate  School  for  the  opportunity  to  further  my 
knowledge  of  engineering,  and  to  Professor  Morris  Driels  for 
his  patience  and  understanding,  and  allowing  me  the  privilege 
of  working  with  him  on  this  project.  Also,  I  wish  to  extend  a 
special  note  of  thanks  to  Jim  Schofield,  Tom  Christian  and  the 
Machine  Shop  for  their  patience  and  technical  help  in  building 
and  testing  the  protype  transducer. 


IX 


I .   INTRODUCTION 

The  main  objective  addressed  in  this  thesis  is  the  design 
and  implementation  of  a  transducer  and  control  system  for  a 
manipulator  which  allows  force  control  to  override  the  normal 
mode  of  rate  control.  For  a  manipulator  operating  in  free 
space,  rate  control  is  sufficient  for  most  tasks.  Use  of  a 
force/torque  sensor  mounted  in  the  wrist  frame  of  a 
manipulator  arm  allows  an  operator  to  perform  certain  tasks  in 
which  contact  forces  develop.  There  are,  however,  inherent 
limitations  in  reliance  on  a  position  sensing  based  controller 
as  a  result  of  the  repeatability  and  accuracy  constraints  of 
any  manipulator  system.  While  it  is  certainly  within  our 
technical  capability  to  develop  more  accurate  sensing  systems 
and  build  more  precise  manipulators,  the  cost  may  very  well  be 
prohibitive  relative  to  the  utility.  It  would  be  far  more 
practical  and  efficient  to  develop  a  means  by  which  to  measure 
and  control  contact  forces. 

For  assembly  tasks  such  as  the  mating  of  two  parts  or 
applying  a  specific  torque  to  a  screw,  rate  control  does  not 
suffice.  A-  simple  example  of  the  utility  of  force  control  is 
illustrated  in  Figure  1  which  shows  the  PUMA  560  Manipulator 
washing  a  window.  If  this  task  were  performed  under  rate 
control,  positional  uncertainty  may  result  in  the  window  being 


broken.  It  would  be  far  more  reasonable  to  specify  a  force  to 
be  maintained  normal  to  the  surface  of  the  glass. 


Figure  1.    PUMA  560  Manipulator  Washing  a  Window  [Ref.  1] 

Although  force-torque  transducers  have  already  been 
developed  using  strain  gauges,  there  is  a  relatively  new 
technology,  known  as  the  Force  Sensing  Resistor01  {FSR)  ,  which 
has  already  been  implemented  in  applications  using  human  touch 
as  an  input.  Successful  implementation  of  an  FSR-based  force- 
torque  transducer  would  prove  to  be  a  far  more  cost  effective, 
electronically  simplified  and  robust  means  of  measuring  and 
controlling  contact  forces  than  the  strain  gauge-based 
transducers  currently  in  use.  Furthermore,  the  FSR  represents 
a  vibrant  new  technology  with  a  host  of  space  related 


applications  such  as  the  Space     Shuttle     Manipulator    Arm. 

Development  of  a  transducer  using  this  technology  and  a 
proposed  method  for  implementation  in  a  force  override  rate 
controller  are  presented. 


II.   PRELIMINARY  WORK 

A.   FORCE  OVERRIDE  RATE  CONTROLLER 

1.  Objectives 

The  purpose  of  the  project  was  to  design  and  prototype 
a  control  system  for  a  remote  actuator  such  as  a  robotic 
manipulator  arm  to  allow  force  control  to  override  the  normal 
rate  control  governing  movement  of  the  manipulator. 

2 .  Background 

Manipulator  systems  such  as  the  Space  Shuttle  RMS  are 
often  operated  remotely  in  rate  control  mode  in  which  a 
constant  controller  input,  such  as  a  fixed  angular 
displacement  of  a  joystick  for  example,  produces  a  constant 
tip  velocity  along  the  trajectory  of  the  manipulator.  When  the 
end-effector  encounters  a  rigid  object,  however,  further 
actuation  is  nullified  when  the  sensed  force  exceeds  an 
operator  specified  threshold.  The  Shuttle  RMS  operates  in  this 
manner  utilizing  a  six  axis  force-torque  sensor  similar  to  the 
model  depicted  in  Figure  2. 

The  shortcomings  of  a  position  sensing  based 
controller  may  be  overcome  through  the  use  of  a  hybrid 
position/force  control  scheme  in  which  the  "shoulder"  and 
"elbow"  joints  are  controlled  with  a  position  controller  and 
the  "wrist"  joints  controlled  with  a  force  controller.  While 
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Figure  2 


Wrist  Mounted  Force/Torque  Sensor  [Ref .  2] 


seemingly  the  ideal  solution,  the  hybrid  controller  must  be 
capable  of  implementing  and  mixing  the  position  and  force 
control  modes  and  may  become  quite  cumbersome.  It  would  be 
more  convenient  as  well  as  more  efficient  if  the  individual 
joints  could  be  operated  in  unilateral  force  control  mode  so 
that  operator  applied  forces  could  be  reflected  at  the 
manipulator  tip.  The  following  section  illustrates  in  simple 
fashion  the  implementation  of  a  remotely  actuated  force 
controller. 

3.   Technical  Approach 

To  demonstrate  the  basic  approach,  consider  the  one 
degree  of  freedom  actuation  system  depicted  in  Figure  3.  In 


Figure  3.   Single  DOF  Hydraulic  Actuation  System 


response  to  an  angular  displacement  of  the  joystick,  the  servo 
valve  allows  hydraulic  fluid  to  flow  to/from  the  hydraulic 
cylinder,  thus  controlling  the  velocity  of  the  ram.  Force 
sensing  is  accomplished  by  means  of  strain  gauges  or  force 
sensing  resistors  {FSR)  mounted  on  the  tool.  The  spring 
attached  to  the  mass  is  used  in  the  control  loop  to  simulate 
compliance  of  the  external  constraint. 

Control  of  this  actuation  system  is  illustrated  by  the 
simple  control  loop  in  Figure  4  in  which  the  open  switch 
represents  the  joystick  in  the  vertical  position.  The  quantity 
Fd  is  a  force  threshold  set  by  the  operator  using  a 
potentiometer  at  which  control  shifts  from  rate  mode  to  the 
force  control  mode.  When  the  joystick  is  displaced  by  some 
angle  /?,  the  switch  is  closed  and  a  voltage  command  is 

supplied  to  the  actuator  causing  the  tool  to  move  with 
velocity  x  until  the  tool  comes  in  contact  with  the  mass- 
spring.  Actuator  movement  ceases  when  the  measured  force 
signal  is  equal  to  Fd.  If  additional  force  is  applied  to  the 
joystick  the  force  sensors  mounted  on  the  stick  increase  the 
forward  path  signal  causing  further  motion  of  the  hydraulic 
ram  and  increasing  the  force  applied  to  the  external 
constraint  by  the  tool.  Thus  the  operator  has  the  capability 
to  reflect  forces  applied  to  the  joystick  at  the  tip  of  the 
actuation  system  in  a  proportional  manner. 
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Figure  4 .   Force  Override  Rate  Control  Loop 

4.   Control  Analysis 

This  system  may  be  modelled  by  the  block  diagram  in 
Figure  5  for  computer  simulation  where  ke  represents  the  servo 
gain  and  Jca  the  actuator  gain;  xe  is  the  linear  position  of  the 
tip  of  the  tool  with  respect  to  some  reference  frame  and  x^  is 
the  linear  deflection  of  the  mass-spring.  The  gain  kx  is  a 
stiffness  ratio  relating  the  stiffnesses  of  the  mass-spring 
and  the  tool.  Summing  forces  yields  the  following  equation: 


►SFX  =  kb{xe-xm)   -  ksKm  =  0 


(1) 


Manipulation  of  Equation  (1)  results  in  the  following 


xm  = 


X, 


(kb+ks)     e 


(kh  >  kj 


(2) 


where  kb  denotes  the  tool  stiffness  and  the  ratio  of  spring 
constants  is  defined  as  the  stiffness  ratio  kr.  The  transfer 
function  that  appears  in  the  forward  path  is  an  electronic 
first  order  servo  lag  with  selectable  time  constant  x.     The 

gain  constant  denoted  by  kg  is  a  feedback  transfer  function 
relating  the  displacement  of  the  end-effector  and  the  voltage 
output  for  strain  gauge  amplifiers. 
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The  block  diagram  in  Figure  5  may  be  simplified  by 
performing  a  few  simple  block  diagram  algebraic  operations. 

Ax  =  xe  -  xm  (3) 

Xm    =   KrXe  <4> 

Substituting  Equation  (4)  into  (3) 

Ax  =  xe  -  krxe  =  xe{l-kr)  (5) 

Using  Equation  (5)  ,  the  second  summing  junction  may  be 
eliminated  and  the  block  diagram  modified  to  the  simpler  form 
in  Figure  6,  where  the  output  and  the  closed  loop  transfer 
function   are  given  by  Equations  (6)  and  (7)  respectively. 


**{s)   =  T^WF(S)  (6) 


x  ~KsKa 

e  x  (7) 


S2    +    7S   +    ±kskakg[l-kr) 


The  denominator  portion  of  Equation  (7)  represents  the 
characteristic  equation  for  the  system,  indicating  a  second 
order  system  of  the  form 

s2  +   25o)ns  +  «>l  (8) 

where 

o)J  =  ijyr^d-jg  (9) 

and  the  damping  ratio   is 

10 


«  = 


(10) 


(2cont) 

The  actuator  gain  and  stiffness  ratio  denoted  by  fca  and  kx 
respectively  are  fixed  values.  The  servo  gain,  kg  and  the 
feedback  transfer  function  gain,  kq  are  variables  and  their 
optimal  values  may  be  determined  by  the  pole  placement  method. 


Figure  6.    Modified  Controller  Block  Diagram 


5.   Simulation  and  Testing 

Using  the  results  of  the  previous  section,  operational 
performance  was  simulated  using  MATRIXx  and  MATLAB.  A  unit 
step  function  was  used  to  simulate  the  joystick  input  and  a 
nominal  value  of  0.1   was  selected  for  the  time  constant  t.  The 
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following  table  gives  the  assumed  values  for  the  gain 
constants.  Using  these  values,  the  simulated  response  was 
characteristic  of  a  second  order  system  as  depicted  in  Figure 
7  and  observed  in  experimental  testing.  The  result  was 
confirmed  in  tests  using  the  PUMA  560  Manipulator. 
TABLE  1 .   Assumed  Gain  Constants 


Name 

Symbol 

Value 

Servo  Gain 

K 

15  v/v 

Actuator  Gain 

K 

2  m/s/v 

Stiffness  Ratio 

K 

0.9  v/m 

Feedback  T.F.  Gain 

K 

2 

Figure  7 .    Controller  Response  to  Step  Input 
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III.   SENSOR  DEVELOPMENT 

A .   THEORY 

1 .   Force  Sensing 

Many  operations  performed  by  a  robot  manipulator  arm 
require  some  means  of  force  sensing  capability.  Some  type  of 
force-torque  sensor  is  typically  mounted  in  the  wrist  frame 
between  the  tip  of  the  manipulator  arm  and  the  end-effector. 
For  a  robot  arm  that  is  tasked  with  gripping  and  transporting 
fragile  objects  or  washing  a  window  for  example,  the  ability 
of  the  robot  to  sense  the  applied  force  is  critical  in  nature. 
Most  wrist  sensors  "sense"  forces  and  torques  applied  at  the 
end  effector  by  transforming  the  applied  forces/moments  into 
measurable  deflections  or  displacements.  The  wrist  sensor 
depicted  in  Figure  2  uses  eight  pairs  of  strain  gauges  wired 
to  a  potentiometer  circuit  to  determine  three  components  of 
force  F  and  three  moment  components  M  by  producing  output 
voltages  proportional  to  the  applied  force/moment. 
Premultiplying  this  sensor  data  matrix  by  the  resolved  force 
matrix  produces  three  orthogonal  force  and  three  torque 
components.  This  assumes  that  all  forces/moments  can  be 
decoupled,  that  strain  gauge  readings  vary  linearly  for  the 
applied  force,  and  that  temperature  effects  are  negligible.  As 
described  by  Fu  [Ref.  3],  eight  voltage  readings  result  from 
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elongation  of  the   strain  gauges.   The   force  vector   is 
determined  from 


where 


F  -  RFW 


F  =    {forces,  moments)7,  =   [Fx,   Fyl   Fz,  Mx,  My,  MZ)T 


W=  zaw  readings  =   {wlt   w2,   w2,    .    .    .    ,wQ 


(11) 


and 


RF  = 


rll 

ri2 

ri3 

^18 

^21 

• 

• 

• 

^31 

■ 

• 

■ 

^61 

, 

, 

r68 

(12) 


The  non-zero  elements  of  the  RF  matrix  are  conversion  factors 
to  convert  voltages  to  units  of  force  or  torque.  In  reality, 
it  is  highly  likely  that  some  coupling  will  exist  and  none  of 
the  48  elements  will  be  zero.  Thus,  in  order  to  eliminate  any 
existing  coupling  and  obtain  accurate  force/moment  readings, 
the  force  matrix  must  be  calibrated  such  that 


and 


W  =  RpalF 


RF     RF   s  J8 


(13) 


(14) 


where  Ie  is  an  identity  matrix  and  the  calibration  matrix  is 
found  by  applying  known  weights  along  the  axes  of  the  sensor 
coordinate  frame.  Because  RF    is  a  non-square  matrix  it  is 
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necessary  to  utilize  the  pseudoinverse .  Premultiplying  a  non- 
square  matrix  by  its  transpose  produces  a  square  matrix  which 
if  it  is  full  rank,  may  then  be  inverted  such  that 

F  =    [  (i?/ai)  T  R£al]  -1  (i?/aJ)  T  W  (15) 

2.   Force  Sensing  Resistors 

Though  strain  gauges  have  been  successfully  applied  in 
the  design  of  wrist  force  sensors  {or  transducers) ,  there  are 
disadvantages  to  using  them,  the  principle  ones  being  the 
requirement  for  the  part  on  which  they  are  mounted  to  be 
capable  of  deflecting,  precise  interface  electronics  and 
relatively  high  cost. 

The  Force  Sensing  Resistor331  (FSR)  though  not  as 
accurate  as  strain  gauges,  are  more  rugged,  significantly  less 
expensive  on  a  per  unit  cost  basis  and  simple  in  design.  They 
are  thin  (on  the  order  of  0.008  to  0.030  inches)  and  composed 
of  a  semi-conducting  polymer  and  conductive  fingers  sandwiched 
between  two  sheets  of  polymer  (Figure  8a) .  Because  of  their 
purely  resistive  impedance,  interface  electronics  are 
simplified.  These  devices  have  no  moving  parts  and  exhibit  a 
decrease  in  resistance  with  increasing  normal  force  applied  to 
the  active  surface  as  shown  in  Figure  8b.  Additional 
characteristics  include  a  large  dynamic  range  [IkO  to  10MQ) , 

insensitivity  to  vibration,  temperature,  chemical  and  moisture 
resistance.  The  FSR  is  also  not  susceptible  to  hysteresis. 
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Figure  8.    FSR  and  Characteristics  [Ref.  4 


Though  not  suited  for  precision  measurement,  implementation  of 
FSR™   technology  into  a  manipulator  force-torque  transducer 
presents  the  opportunity  for  a  more  robust,  less  expensive  and 
electronically  simplified  sensor. 
3.   Transducer  Design 

a.  Preliminary  Design 

After  suggesting  an  investigation  into  the 
feasibility  of  FSR  application,  the  project  sponsor  at  NASA 
proposed  a  conceptual  design  in  which  individual  sensors  are 
mounted  on  a  cube  which  is  then  embedded  in  a  semi-compliant 
material  like  RTV.  Figure  9  illustrates  the  preliminary  design 
that  served  as  the  conceptual  basis  on  which  transducer 
development  proceeded. 
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Figure  9.    FSR-Based  Transducer  Conceptual  Design 

b.    Sensor  Placement 

The  first  question  that  needed  to  be  addressed  was 
the  placement  of  sensors.  Assuming  the  sensors  are  mounted  on 
an  aluminum  cube,  how  many  sensors  are  required  to  be  able  to 
detect  six  force  components  (positive  and  negative)  and  six 
moments?  Mathematically,  the  problem  may  be  generalized  using 
a  matrix  equation  in  the  form  of  Equation  (16) 
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where  S  is  an  nxl  sensor  output  matrix  and  A  an  nxl2  sensor 
coefficient  matrix.  Because  the  FSR  only  responds  to  a 
compressive  force,  sensors  must  be  strategically  located  to 
determine  both  positive  and  negative  force/torque  components. 
Hence,  F  is  a  12x1  force-torque  component  matrix.  The  question 
is:  what  is  n  and  how  are  the  sensors  placed  so  that  all  force 
components  and  moments  may  be  identified?  To  utilize  the 
pseudoinverse,  the  matrix  algebra  dictates  that  the  number  of 
sensors  must  be  greater  than  or  equal  to  the  number  of 
force/torque  components  to  be  determined.  If  n  is  12  then  the 
force  components  may  be  determined  from 


F  =   A_15 


(17) 


If  the  number  of  sensors  is  greater  than  twelve  then  A   cannot 
be  inverted  by  itself  being  non-square.  Premultiplying  both 
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sides  of  Equation  (16)  by  AT  produces  a  square  matrix  ArA.  If 
this  matrix  is  full  rank  it  may  be  inverted  so  that 

F   =  [ATA)'XATS  <18> 

Rank  deficiency  of  this  matrix  implies  that  all  components  of 
F  are  not  identifiable.  The  basic  problem  then  is  one  of 
determining  the  minimum  number  of  sensors  required  to  detect 
all  force/moment  components  and  to  determine  where  on  the  cube 
these  sensors  need  to  be  placed  to  identify  all  force/moment 
components . 

4 .   Development  of  Sensor  Equations 

Initial  analysis  was  performed  by  modelling  the 
sensor-transmitter  as  a  planar  joystick  attached  by  springs  to 
a  rigid  surface.  Using  the  principle  of  superposition, 
equations  were  developed  for  each  of  the  sensors  to  determine 
which  sensors  are  activated  when  a  particular  force  or  moment 
is  applied.  Referring  to  Figure  10,  the  force  applied  to 
sensor  number  one  for  an  operator  applied  force  P  is  composed 
of  components  due  to  translation  and  rotation  of  the  cube 

P  M 

F     =  £.   +  JL  where  M  =  Pd  (19) 

Sl   4   4  a 

Making  the  indicated  substitution,  Equation  (19)  may  be 
rewritten  in  more  convenient  form  as 

Fs    =  Z{i  +  A)    >  o  (20) 

*i   4     a 
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The  quantity  in  parentheses  indicates  that  a  compressive  force 
is  applied  to  the  sensor,  thus  it  will  produce  an  electrical 
output.  Using  the  same  methodology  for  the  remaining  sensors, 
the  following  equations  apply. 


Fq    =   —  (1  -  — )  <0 


(21) 


Fq    =  _(-l  +  -)  >  0 


(22) 


Fq    =  —{-1   -   —)    <  0 


(23) 


For  a  pure  moment  M   about  the  Y   axis 


F0  =  Fc 


M 

4a 


(24) 


Figure  10.   Planar  Joystick 
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Noting  from  Figure  10  that  d/a  >  1,  it  is  apparent 
from  the  above  equations  that  for  an  applied  force  P,  sensors 
1  and  3  are  in  compression  while  sensors  2  and  4  are  in 
tension.  Because  only  a  compressive  force  produces  an  the  FSR 
response,  the  a^  for  sensors  2  and  4  due  to  P  are  zero.  It  is 
this  methodology  that  is  used  hereafter  to  determine  the 
active  sensors  for  a  given  force  or  moment  and  to  determine 
the  sensor  coefficient  matrix  A,  where  a±j  are  computed  by  the 
applicable  general  formulae 

a±i      =  C(±—   +  — )  (25) 

1J<*>  nt        nTa 

a-      =  C(-i-)  (26) 

1J<">  nra 

where,  considering  individual  FSR's  to  be  represented  by 
springs,  nt  is  the  number  of  springs  which  deflect  due  to 
translation  of  the  cube;  nr  is  the  number  of  springs  which 
deflect  due  to  pure  rotation  of  the  cube;  and  C  is  some 
arbitrary  scaling  constant  such  that  the  coefficient  matrix  is 
wholly  comprised  of  integers. 
5 .  Sensor  Placement 
a.  Methodology 

The  first  several  attempts  to  solve  the  problem  of 
sensor  placement  were  approached  in  an  intuitive  manner. 
Building  on  the  planar  case  of  the  previous  section,  four  more 
sensors  were  added  as  depicted  in  Figure  11.  Referring  to  the 
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planar  joystick  procedure  of  the  previous  section,  the  same 
principle  of  determining  the  active  sensors  for  a  given 
force/moment  applies.  The  tabular  format  in  Table  2  was  used 
for  the  various  sensor  configurations  to  determine  the  sensor 
coefficient  matrix.  A  d/a  ratio  of  5  was  assumed  with  a  equal 
to  1. 


Figure  11.   Transducer  with  8  Sensors 
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TABLE  2.   BIPLANAR  TRANSDUCER  COEFFICIENT  ARRAY 


Sx 

s2 

s3 

s4 

s5 

s6 

s7 

S8 

Fx+ 

6/4 

0 

0 

1 

0 

0 

0 

0 

Fx- 

0 

1 

6/4 

0 

0 

0 

0 

0 

V 

0 

0 

0 

0 

6/4 

0 

0 

1 

Fy" 

0 

0 

0 

0 

0 

1 

6/4 

0 

Mx+ 

0 

0 

0 

0 

1/4 

0 

0 

1/4 

M," 

0 

0 

0 

0 

0 

1/4 

1/4 

0 

*V 

0 

1/4 

1/4 

0 

0 

0 

0 

0 

My" 

1/4 

0 

0 

1/4 

0 

0 

0 

0 

The  coefficient  matrix  A   is  the  transpose  of  the  8x8   array  in 
Table  2 


A   = 


60000001 
04000010 
06000010 
40000001 
00601000 
00040100 
00060100 
00401000 


(27) 


where  the  matrix  has  been  multiplied  by  a  constant  {4)  for 
convenience.  S  is  given  by  the  transpose  of  the  first  row  and 
F  by  the  first  column  of  Table  2.  If  A  is  full  rank  then  F  may 
be  calculated  by 
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F   =  A^S  (28) 

Using  the  software  program  MATLAB,  the  rank  was  calculated  to 
be  8,  indicating  that  all  force  and  moment  components  may  be 
determined  using  Equation  (28).  Attempting  to  determine 
additional  force  or  moment  components  with  this  sensor 
configuration  is  not  possible  because  the  minimum  number  of 
sensors  must  be  equal  to  or  greater  than  the  number  of 
force/moment  components  to  be  determined. 

Having  mathematically  proven  the  ability  to 
determine  eight  force  and  moment  components  with  this  sensor 
configuration,  the  next  step  was  to  add  four  more  sensors,  two 
to  both  the  positive  and  negative  Z  faces  in  the  same  diagonal 
pattern  as  illustrated  in  Figure  12.  Taking  the  transpose  of 
the  12x12  array  in  Table  3  yields  the  A  matrix  in  Equation 
(29) .  From  MATLAB,  the  rank  of  this  matrix  is  10,  indicating 
that  all  force  and  moment  components  cannot  be  identified. 

Numerous  attempts  to  intuitively  solve  the  rank 
deficiency  problem  by  repositioning  the  sensors  proved 
unsuccessful,  so  a  new  approach  was  taken.  Nine  sensors  were 
placed  on  each  face  of  the  cube  for  a  total  of  54  sensors  as 
illustrated  in  Figure  13.  For  a  force  P  applied  to  the 
joystick  the  sensor  output  is  composed  of  components  due  to 
translation  and  rotation  of  the  cube.  The  component  due  to 
translation  for  a  single  sensor  is  P/18.   The  component  due  to 
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Figure  12.   Six  Axis  Transducer 
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TABLE  3.  SIX  AXIS  TRANSDUCER  COEFFICIENT  ARRAY 


Sx 

s2 

s3 

s4 

Ss 

S6 

s7 

s8 

s9 

s10 

Sn 

s12 

F  + 

7 

0 

0 

3 

0 

0 

0 

0 

0 

0 

0 

0 

F  " 

0 

3 

7 

0 

0 

0 

0 

0 

0 

0 

0 

0 

V 

0 

0 

0 

0 

7 

0 

0 

3 

0 

0 

0 

0 

fy" 

0 

0 

0 

0 

0 

3 

7 

0 

0 

0 

0 

0 

F2+ 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

2 

2 

F2" 

0 

0 

0 

0 

0 

0 

0 

0 

2 

2 

0 

0 

*C 

0 

0 

0 

0 

1 

0 

0 

1 

0 

1 

1 

0 

Mx" 

0 

0 

0 

0 

0 

1 

1 

0 

1 

0 

0 

1 

V 

0 

1 

1 

0 

0 

0 

0 

0 

1 

0 

0 

1 

*v 

1 

0 

0 

1 

0 

0 

0 

0 

0 

1 

1 

0 

Mz+ 

0 

1 

0 

1 

0 

1 

0 

1 

0 

0 

0 

0 

M," 

1 

0 

1 

0 

1 

0 

1 

0 

0 

0 

0 

0 

A  = 


7 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

1 

0 

3 

0 

0 

0 

0 

0 

0 

1 

0 

1 

0 

0 

7 

0 

0 

0 

0 

0 

0 

1 

0 

0 

1 

3 

0 

0 

0 

0 

0 

0 

0 

0 

1 

1 

0 

0 

0 

7 

0 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

0 

3 

0 

0 

0 

1 

0 

0 

1 

0 

0 

0 

0 

7 

0 

0 

0 

1 

0 

0 

0 

1 

0 

0 

3 

0 

0 

0 

1 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

2 

0 

1 

1 

0 

0 

0 

0 

0 

0 

0 

0 

2 

1 

0 

0 

1 

0 

0 

0 

0 

0 

0 

2 

0 

1 

0 

0 

1 

0 

0 

0 

0 

0 

0 

2 

0 

0 

1 

1 

0 

0 

0 

(29) 
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Figure  13.   54  Sensor  Transducer 
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rotation  for  a  single  sensor  is  M/24a  where  the  sensors  along 
the  axis  of  rotation  are  assumed  to  see  neither  a  compressive 
or  tensile  force.  Likewise,  the  output  of  a  single  sensor  due 
to  a  pure  moment  or  torque  (M)  is  M/24a.  This  results  in  a 
slight  modification  of  the  basic  planar  joystick  sensor 
equations.  For  the  illustrated  example  of  a  force  applied  in 
the  positive  X   direction 

Fs         =   _£  +   *L  (30) 

5i'2-3   18    24a 


Fs         .  -_L  ♦  _2L  (3D 

^16,17,18    is    24a 


FS           =    TS  (32) 

which  if  multiplied  by  a  convenient  constant  (72)  become 

F          =   p(4  +  M)  (33) 
^1-2. 3          a 


^16  ,  17  ,  18 


=  p(-4  +  M)  (34) 


F.S6  =   P(4)  (35) 

4  i  5  f  o 


For  a  pure  moment  about  the  -Y   axis 

Fs    =  Fs  =  -JL  (36) 

al,2,3       D16,17,18      243 

which  when  multiplied  by  the  same  constant  becomes 
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Fs         =  Fs  =  M  (37) 

Dl,2,3  •316,17,18  Q 

Utilizing  Equations  (33)  through  (35)  and  (37)  yields  the 
54x12  A  matrix  given  in  Appendix  A.   Premultiplying  this  matrix 
by  its  transpose  and  using  MATLAB   to  compute  the  rank  of  ArA 
resulted  in  a  full  rank  coefficient  matrix  {12) . 
b.    Transducer  Design  Optimization 

Having  proven  the  solvability  of  this  problem  using 
54  sensors  and  knowing  the  minimum  number  of  sensors  to  be  12, 
the  next  logical  step  was  to  attempt  to  optimize  the  design  by 
eliminating  unnecessary  sensors,  if  any.  Rather  than 
eliminating  sensors  one  at  a  time,  a  selective  elimination 
pattern  was  used  to  eliminate  groups  of  sensors  from  each 
face.  The  steps  of  this  trial  and  error  process  are  detailed 
in  Appendix  A.  This  elimination  process  was  performed  without 
making  any  modifications  to  the  basic  sensor  equations.  The 
end  result  was  achievement  of  the  global  minimum  for  the 
number  of  sensors.  It  should  be  noted  that  the  resulting 
placement  pattern  is  not  a  unique  solution,  but  rather  a 
particular  solution.  A  different  sensor  elimination  strategy 
may  well  result  in  a  different  placement  pattern.  The 
placement  of  sensors  is  depicted  in  Figure  14  and  the 
unmodified  coefficient  matrix  given  by  Equation  (38) .  As  a 
check,  a  modified  A  matrix  which  is  based  on  12  rather  than  54 
sensors  was  computed  (Equation  39)  with  the  rank  remaining  12. 
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Figure  14.   Optimized  Full  Order  Transducer 
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A  = 


19  0  0  0  00000303 

0  110  0  00003030 

0  19  0  0  00003030 

11  0  0  0  00000303 

0  0  19  0  00300003 

0  0  0  11  00030030 

0  0  0  19  00030000 

0  0  0  4  00000003 

0  0  0  400000030 

0  0  11  0  00300000 

15  0  0  15  04030300 

15  0  15  0  40300300 


(38) 


A  = 


26     0  0  0  0  0000403 

0     14  0  0  0  0004030 

0     26  0  0  0  0004040 

14     0  0  0  0  0000403 

0      0  24  0  0  0400003 

0      0  0  16  0  0040030 

0      0  0  24  0  0040000 

0      0  0  4  0  0000003 

0      0  0  4  0  0000030 

0      0  16  0  0  0400000 

20     0  0  20  0  12    040400 

20     0  20  0  12  0400400 


(39) 


c.  Reduced  Order  Transducer 

Not  all  manipulator  tasks  utilizing  force  control 
require  a  full  order  force-torque  transducer.  Having 
calculated  one  feasible  placement  strategy,  the  next  step  was 
to  determine  the  optimal  placement  for  a  reduced  order 
transducer  capable  of  detecting  three  forces  (in  both  the 
positive  and  negative  directions)  and  a  moment  (positive  and 
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negative)  about  the  Z  axis.  Once  again  sensors  were  eliminated 
on  a  trial  and  error  basis  until  further  elimination  of  any 
sensors  resulted  in  a  rank  deficient  coefficient  matrix.  From 
the  derived  sensor  pattern  a  reduced  order  prototype  was 
designed  with  the  sensor  placement  pattern  depicted  in  Figure 
15.  The  following  equation  applies  to  the  reduced  order 
transducer.  Equation  (41)  gives  the  modified  A   matrix. 


Si 

s2 

S3 

£4 

s5 

= 

s6 

Si 

s8 

19 

0 

0 

0 

0 

0 

0 

3 

0 

19 

0 

0 

0 

0 

0 

3 

0 

0 

19 

0 

0 

0 

0 

3 

0 

0 

0 

19 

0 

0 

0 

0 

0 

0 

0 

4 

0 

0 

0 

3 

0 

0 

0 

4 

0 

0 

3 

0 

0 

15 

15 

0 

0 

4 

0 

0 

0 

15 

0 

15 

4 

0 

0 

0 

Ml 


m: 


(40) 


A   = 


55 

0 

0 

0 

0 

0 

0    4 

0 

35 

0 

0 

0 

0 

0    4 

0 

0 

30 

0 

0 

0 

0    4 

0 

0 

0 

30 

0 

0 

0    0 

0 

0 

0 

5 

0 

0 

0    4 

0 

0 

0 

5 

0 

0 

4    0 

0 

5 

5 

0 

0 

10 

0    0 

0 

5 

0 

5 

10 

0 

0    0 

(41) 
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Figure  15.   Reduced  Order  Transducer 
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B.   PROTOTYPE  TRANSDUCER  DESIGN 
1 .   Mechanical  Design 

Some  manipulator  tasking  problems  requiring  force 
control  do  not  require  the  use  of  a  full  order  force-torque 
transducer.  If  for  example,  the  manipulator  is  used  to  apply 
a  specific  torque  to  a  screw,  the  primary  consideration  is  the 
ability  to  measure  the  torque  about  the  z  axis  of  the 
transducer  coordinate  frame.  Using  the  sensor  placement 
pattern  developed  in  the  previous  section  for  the  reduced 
order  transducer  simplifies  the  electronic  interface  problem 
and  successful  implementation  using  the  PUMA  560  will  verify 
the  theory  developed  in  the  previous  section. 

Because  of  the  weight  limitations  of  the  PUMA,  the 
mechanical  design  of  the  prototype  sensor  had  to  be  carefully 
considered.  For  simplicity,  aluminum  stock  materials  were 
used.  The  choice  of  aluminum  minimized  weight  while  satisfying 
the  requirement  that  the  FSR's  be  mounted  on  a  firm  backing. 
The  only  other  constraints  were  the  size  of  the  FSR's  and  the 
wiring  requirements.  An  isometric  drawing  of  the  mechanical 
design  is  depicted  in  Figure  16a.  Engineering  specifics  are 
provided  in  a  plan  view  diagram  of  the  prototype  given  in 
Figure  16b. 

Mounting  of  the  sensors  on  the  aluminum  cube  proved  to 
be  somewhat  of  a  problem  initially.  After  cleaning  the  surface 
of  the  cube  with  acetone  followed  by  alcohol,  M-Bond  200   was 
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used  to  secure  the  sensors.  Upon  completion,  the  sensors  were 
tested  for  response  and  found  to  be  inoperable.  The  epoxy  was 
apparently  incompatible  with  the  substrate  of  the  FSR.  New 
sensors  were  then  mounted  using  a  microthin  double  sided 
adhesive  produced  by  3-M.  After  verifying  proper  response 
characteristics,  the  individual  sensors  were  then  wired  using 
16  wire  ribbon  cable  and  then  electrically  insulated  using  an 
air  curing  3140  sealant.  Table  4  provides  the  wiring  code  for 
individual  sensors. 

TABLE  4.   PROTOTYPE  TRANSDUCER  WIRING  CODE 


Sensor 
Location 

Sensor 
Number 

Mounting 
Face 

Wiring  Color  Code 

1 

1 

+X 

orange/orange 

19 

2 

+  Y 

yellow/yellow 

10 

3 

-X 

green/green 

29 

4 

-Y 

blue/blue 

31 

5 

-Y 

white/black 

33 

6 

-Y 

brown/brown 

37 

7 

-Z 

red/red 

46 

8 

+  Z 

gray/purple 
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(a) 


(b) 


RTV 


Part  1 


Part  2 


3.0" 


318"  Aluminum  Disc 


2.875" 


Aluminum 
Cylinder 


114"  RTV  Sealant 


112"  Aluminum  Disc 


Figure  16.   Prototype  Transducer  Mechanical  Design 
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2.   Transducer  Testing  and  Calibration 

Sensor  leads  were  wired  to  an  analog  to  digital  data 
translation  board  in  a  voltage  divider  configuration  with  a  20 
volt  range  {-10v  to  +10v)  to  increase  sensitivity.  The  data 
board  was  then  connected  to  a  Zenith  286PC  for  data 
acquisition  using  the  program  GRABDAT.  However,  despite 
satisfactory  sensor  response  tests  after  the  mounting  and 
wiring  procedures  and  again  after  sealing  the  sensors 
electrically  with  an  RTV  sealant,  when  the  transducer  was 
setup  for  calibration  two  sensors  failed  to  provide  any 
response.  Two  more  provided  only  intermittent  response 
indicating  the  presence  of  shorts.  Referring  to  Table  4  and 
Figure  15,  the  four  remaining  sensors  with  good  response 
characteristics  were  sensors  1,  4,  5  and  6.  Though  not  capable 
of  detecting  the  six  force  and  two  moment  components  it  was 
designed  to  detect,  the  transducer  is  capable  of  detecting  two 
force  and  two  moment  components  as  shown  in  Table  5. 

TABLE  5.   OPERABLE  SENSOR  COEFFICIENT  ARRAY 


Si 

s4 

s5 

s6 

Fx+ 

(1   +   d/a) 

0 

0 

0 

V 

0 

(1/3   +   d/a) 

1/3 

1/3 

k* 

0 

0 

0 

l/3a 

Ma" 

l/3a 

0 

l/3a 

0 

where  a   is  15/32",  d   is  1.5"  plus  the  length  of  the  arm,  and 
the  coefficient  matrix  A   is  the  transpose  of  the  4x4   array. 
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Calibration  of  the  transducer  prototype  for  these 
components  is  warranted  as  it  will  adequately  validate  the 
concept  for  any  order  transducer.  This  procedure  may  be 
accomplished  by  placing  the  transducer  in  an  indexing  chuck 
and  applying  a  known  weight  in  each  direction  of  the  six  axis 
transducer  coordinate  frame. 
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IV.   FORCE  CONTROL 

A .   THEORY 

1 .  Robotic  Application  of  Transducer 

The  transducer  discussed  in  the  previous  chapter  may 
be  utilized  with  a  robotic  manipulator  arm  such  as  the  PUMA 
560  6R  Manipulator  in  two  different  ways  as  depicted  in  Figure 
17.  First,  it  may  be  used  as  a  joystick  in  force  override 
control  mode  to  reflect  desired  forces  or  torques  in  the  tool 
frame.  Secondly,  it  may  be  attached  in  the  wrist  frame  to 
sense  the  actual  force  or  torque  being  applied  and  fed  back  to 
the  controller  to  produce  a  force  error  signal.  The  following 
sections  provide  the  development  for  general  manipulator 
mechanics  and  a  method  by  which  a  force  error  signal  may  be 
utilized  to  achieve  force  reflection  in  the  end-effector. 

2 .  Manipulator  Kinematics 

a.  Generalized  Coordinate   Transformations 

Given  a  coordinate  reference  coordinate  system  {A} 
with  three  mutually  orthogonal  unit  vectors,  any  point  in 
space  may  be  located  by  a  position  vector  [Ref.  5] 

PA    =  Pj    +  PyJ    +  Pz*  <42> 

When  dealing  with  a  rigid  body  such  as  an  end- 
effector  tool  it  is  necessary  to  know  its  position  and 
orientation,  hereafter  referred  to  as  its  pose,    with  respect 
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Figure  17.   Robotic  Application  of  Transducer 

to  the  reference  coordinate  system.  To  determine  the 
orientation  it  is  necessary  to  attach  a  coordinate  system  to 
the  body  at  some  convenient  location,  for  example,  a  joint  or 
the  center  of  mass.  The  orientation  of  the  unit  vectors  of 
the  body-attached  coordinate  system  with  respect  to  the 
reference  system  is  obtained  by  taking  the  vector  dot  product 
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of  the  unit  vectors.  Writing  the  unit  vectors  of  coordinate 
system  {B}  in  terms  of  the  reference  system  produces  a  3x3 
matrix  known  as  the  rotation  matrix.  Because  this  particular 
matrix  is  orthonormal,  it  is  useful  to  note  that  the  inverse 
of  the  rotation  matrix  is  equal  to  its  transpose  [Ref.  6]. 

A  frame  is  a  set  of  four  vectors  that  represents 
the  pose  of  a  body  with  respect  to  some  reference  system. 
Mathematically,  a  frame  is  a  rotation  matrix  and  a  position 
vector  which  describe  one  coordinate  system  relative  to 
another  [Ref.  7] .  A  vector  whose  description  is  known  in  frame 
{B}  may  be  described  in  frame  {A}  through  a  process  of 
translational  and  rotational  mapping.  Mathematically, 
description  of  the  vector  in  the  {A}  frame  is  accomplished  by 
a  transformation  matrix  operating  on  P®,  given  in  compact 
notation  as 


pa  =  t:p 


BnB 


(43) 


or  in  matrix  form  as 


0  0  0 


BORG 


(44) 


Jb.  Compound  Transformations 

For  a  robotic  manipulator  arm  with  multiple  frame 
assignments,  it  is  necessary  to  perform  compound  coordinate 
frame  transformations  [Ref.  8]  in  order  to  relate  the  pose  of 
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{A} 


Figure  18.    Translated  and  Rotated  Link  Frame 

the  end-effector  to  the  base  frame.  A  generalized  example  of 
this  process  is  depicted  in  Figure  18.  With  the  description  of 
frame  {C}  known  relative  to  frame  {B} ,  vector  P°  can  be 
transformed  into  {B)   by 


pb   =  TBpC 


which  may  then  be  transformed  into  {A}  by 


(45) 


PA    =    TApB 


(46) 
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Combining  Equations  (45)  and  (46)  yields  the  following  result: 

pA  =  tatbpc  (47) 

Generally  stated,  a  vector  in  any  frame  may  be  described  in 
the  reference  frame  by  multiplying  the  vector  by  the  product 
of  individual  transformation  matrices. 

c.  Denavit-Hartenberg  Transformations 

A  manipulator  arm  consists  of  a  series  of  links 
connected  by  revolute  or  prismatic  joints  with  each  link-joint 
pair  constituting  1  degree  of  freedom.  To  describe  the 
relative  pose  of  each  link  in  a  kinematic  chain,  Denavit  and 
Hartenberg  [1955]  proposed  a  systematic,  matrix  method 
approach  to  the  establishment  of  link  coordinate  frames.  The 
Denavit-Hartenberg  {D-H)  method  is  a  convenient  and  popular 
convention  that  results  in  a  4x4  homogeneous  transformation 
matrix  representing  each  link's  coordinate  system  at  the  joint 
with  respect  to  the  previous  link's  coordinate  system.  Thus, 
the  end-effector  frame  for  any  n  degree  of  freedom  manipulator 
may  be  expressed  in  reference  frame  coordinates  through  a 
series  of  sequential  tranformations .  [Ref.  9] 

Links  are  numbered  outwardly  from  the  base  of  the 
manipulator  arm  starting  with  link  0,  which  is  attached  to  the 
base  frame  and  not  considered  part  of  the  system  from  a 
kinematic  standpoint.  Link  n  is  the  point  at  which  a  tool  is 
attached.  Thus,  an  n  degree  of  freedom  manipulator  has  n+1 
coordinate   frame  assignments.   The  initial   frame  {0}      is 
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assigned  to  a  convenient  location  on  link  0,  often  referred  to 
as  the  base  frame  {B} ,  and  serves  as  an  inertial  reference 
frame  for  the  manipulator  arm. 

Referring  to  Figure  19,  the  following  general  rules 
apply  to  the  assignment  of  individual  link  coordinate  frames 

1.  The  z^.t    axis  lies  along  the  axis  of  motion  of  the  1th 
joint . 

2.  The  x±   axis  is  normal  to  the  z±.x   axis,  and  pointing  away 
from  it. 

3.  The  y±   axis  completes  the  right-handed  coordinate  system 
as  required. 

Associated  with  each  link  in  the  kinematic  chain  of 
a  manipulator  for  1  =  1,  2,...,  n,  are  two  sets  of  parameters; 
the  joint  parameters  (d^  and  0±)    which  describe  the  relative 

positions  of  neighboring  links  and  the  link  parameters  {a±  and 
aj  which  physically  describe  the  link.  To  obtain  these 

parameters  a  joint  axis  is  established  for  joint  i  as  depicted 
in  Figure  18.  Because  the  joint  connects  two  links,  the  joint 
axis  has  two  normals.  The  relative  position  of  link  i  and  link 
1-1  along  the  common  joint  axis  is  known  as  the  link  offset 
and  denoted  by  d^.  This  offset  is  a  variable  for  a  prismatic 
joint.  The  joint  angle  6±   is  the  degree  of  rotation  about  the 

common  axis  between  neighboring  links.  For  a  revolute  joint, 
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X/-1 


Link  /  +  1 


Figure  19.  Generic  Manipulator  Link  [Ref.  10 


this  parameter  is  a  variable.  The  linJc  length  a±  is  the 
distance  along  the  common  normal  between  joint  axes.  The  link 
twist    a±    is  the  angle  between  the  joint  axes  measured  in  a 

plane  perpendicular  to  a±.  Referring  to  Figure  18,  the 
convention  for  measuring  these  parameters  is  as  follows. 
[Ref.  11] 


ft 


is  the  distance  from  the  origin  of  the  (i-1)  **  coordinate 
frame  to  the  intersection  of  the  zx_t   axis  with  the  x±   axis 
along  the  z±_x   axis, 
is  the  joint  angle  from  the  xx_x   axis  to  the  x2   axis  about 

the  z±.±   axis  (using  the  right-hand  rule)  . 


45 


a±       is  the  offset  distance  from  the  intersection  of  the  z^ 
axis  with  the  x±  axis  to  the  origin  of  the  1th   frame  along 
the  x±   axis. 

a±      is  the  offset  angle  from  the  z±.t   axis  to  the  zL   axis  about 

the  jca  axis  (using  the  right-hand  rule)  . 

Again,  referring  to  the  diagram  for  a  general  link 
provided  in  Figure  18,  it  may  be  shown  that  a  point  in  the  1th 
frame  may  be  expressed  in  the  (i-l)**  frame  by  performing  a 
series  of  four  transformations,  symbolically  represented  in 
Equation  (48)  .  The  product  of  these  transformations  is  a 
composite  homogeneous  transformation  matrix  for  adjacent  link 
coordinate  frames  and  is  known  as  the  D-H  transformation 
matrix,    given  by  Equation  (47).  [Ref.  13] 


rpl~~i       rp  rp  rp  rp 

1i  "    1z,   d1z,  Q1x,   a1x,  a 


(48) 


n 


i-l 


cosGj   -cosctjSinGj    sinc^sinGj    aicos0i 
sinGj    cosaicos0i     -sinoCjCosGj   a^inGj 


since 


cosa 


(49) 


d.  PUMA  560  Denavit-Hartenberg  Parameters 

Within  the  bounds  of  the  rules  for  link  frame 
allocation  previously  stated,  different  authors  have  slightly 
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different  conventions  for  the  assignment  of  these  frames.  The 
method  applied  by  Fu  [Ref.  12]  is  adopted  here.  Link  frame 
assignments  for  the  PUMA  560   are  given  in  Figure  20. 


Figure  20.  PUMA  Link  Frame  Assignments 


TABLE  6.  PUMA  560  LINK  AND  JOINT  PARAMETERS 


Joint   i 

0< 

0, 

a<    (mm) 

d)    (mm) 

1 

90 

-90 

0 

0 

2 

0 

0 

431.8 

149.09 

3 

90 

90 

-20.32 

0 

4 

0 

-90 

0 

433.07 

5 

0 

90 

0 

0 

6 

0 

0 

0 

56.25 

e.  PUMA   560  Forward  Kinematics 

Kinematics  in  general  is  the  study  of  motion 
without  regard  for  the  forces  or  torques  required  to  produce 
that  motion.  The  manipulator  forward  kinematic  problem 
involves  the  concatenation  of  neighboring  link  transforms  to 
solve  for  the  pose  of  a  specific  link  with  respect  to  the  base 
frame  and  thus,  describes  how  the  relative  pose  of  assigned 
link  coordinate  frames  change  with  respect  to  an  inertial 
reference  frame  as  the  manipulator  articulates  through  space. 

In  the  case  of  the  PUMA  560,  the  pose  of  frame  {6} 
with  respect  to  {0}  may  be  found  by  multiplying  the  following 
kinematic  chain  of  transformation  matrices 


Te  =  7,1°r21r32r43r54r65 


(50) 


Successive  multiplication  of  4x4  matrices  can 
become  quite  cumbersome  algebraically.  In  order  to  simplify 
this  task,  it  is  convenient  to  multiply  the  first  and  second 
three  transformation  matrices  to  form  two  separate  matrices 
such  that 


fTiO       _       mOT3       _ 

1e    ~   i3 Ie    - 


ril  ri2  ri3  Px 

r21  r22  r23  Py 

r31  r32  r33  Pz 

0  0  0  1 


(51) 
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in  which  the  3x1  submatrix  (px  py  pz)T  represents  the  position 
of  the  end-effector  and  the  3x3  rotation  submatrix  represents 
the  orientation.  The  12  equations  that  result  form  the  upper 
three  rows  of  the  matrix  constitute  the  forward  kinematics  of 
the  PUMA  560  manipulator.  Individual  transformation  matrices 
and  the  kinematic  equations  are  provided  in  Appendix  B. 
f.    Inverse  Manipulator  Kinematics 

Whereas  the  forward  kinematic  problem  is  solely 
concerned  with  computing  the  pose  of  the  end-effector  with 
respect  to  an  inertial  reference  or  base  frame,  the  inverse 
kinematic  problem  in  concerned  with  computation  of  the 
required  set  of  joint  angles  to  produce  the  desired  pose  of 
the  end-effector.  Recall  that  the  pose  of  the  PUMA  end- 
effector  with  respect  to  the  base  frame  is  given  by  the 
manipulator   transformation  matrix. 


T?   = 


ril  ri2  ri3  Px 

•r21  r22  r23  Py 

r31  r32  r33  Pz 

0  0  0         1 


=   T?  (0J  Tl  (02)  Tl  (03)  Tl  (04)  Tl  (05)  Ti  (06) 


(52) 


The  resulting  kinematic  equations  are  nonlinear, 
transcendental  equations  that  at  best  are  difficult  to  solve. 
From  Equation  (52)  there  are  twelve  equations  and  six  unknown 
joint  angles,  indicating  the  possibility  of  multiple  solutions 
[Ref.  14].  The  inverse  transform   technique   for  obtaining  the 
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joint  angles  involves  sequential  inversion  of  the  individual 
transformation  matrices  and  equating  matrix  elements  to  obtain 
a  trigonometric  relationship  for  the  individual  joint  angle 
(numerical  methods  may  also  be  employed) .  For  example,  to 
solve  for  &x 

[^(Gjr1  r6°  =  T21(02)r32(03)T43(e4)r54(05)T65(06)     (53) 

where  the  indicated  transformation  matrix  inverse  can  be  found 
from 


[rf1]"1  =  tU  = 


cos0i  sinGj  0  -aL 

-cosctisin0i    cosaicos0i    sintXj  -c^sincCj 

sinaisin0i     -sinc^cosOj  cosaj  -c^coscCj 
0                         0                   0  1 


(54) 


rewriting  Equation  (53 


COS0-L  sinOj^    0     0 

0  0-10 

-sin01  cos©!    0     0 

0  0         0     1 


ril  ri2  ri3  Px 

r21  r22  r23  Py 

r31  r32  r33  Pz 

0          0          0  1 


(55) 


Performing  the  indicated  matrix  multiplication  and  following 
the  methodology  outlined  by  Craig  [Ref.  15]  of  equating 
elements  from  both  sides  of  Equation  (55)  and  making  use  of 
several  trigonometric  substitutions  and  identities,  a  solution 
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may  be  obtained  for  8X.  Joint  angles  G2  through  06  are  obtained 
in  a  similar  manner. 

3 .   Jacobian  Development 

a.  Static  Forces  in  Manipulators 

As  with  any  other  static  structure  the  forces  and 
moments  in  each  link  must  balance  at  any  given  instant.  To 
determine  the  static  forces  in  the  manipulator  it  is  necessary 
to  write  force  and  moment  equations  for  each  individual  link. 
From  these  equations  the  required  joint  torque  for  each  link 
to  maintain  static  equilibrium  may  be  computed.  [Ref.  16] 
Figure  20  depicts  the  free  body  diagram  for  a  generic  link 


Figure  21.    Forces  on  a  Manipulator  Link 
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where  f±  and  n^  are  force  and  torque  respectively  exerted  on 
link  i  by  link  i-1.  Summing  forces  and  moments  and  setting 
equal  to  zero  yields  the  following  results: 

ft   =  fU  (56) 

nl  =  nU  +    PU   x  fU  (57) 

These  equations  may  be  written  in  terms  of  only  forces  and 
moments  within  their  own  link  coordinate  frames  by  multiplying 
the  right  side  of  each  equation  by  the  rotation  matrix  that 
describes  link  frame  {±+1}  relative  to  {1} .  These  equations 
are  applied  starting  from  the  last  link  of  the  manipulator  and 
proceeding  inward. 

ft  -  rL  fi:i  <58> 

nt  -  RU  nill  +  PU  x    ft  (59) 

The  joint  actuator  torque  required  to  maintain 
static  equilibrium  is  obtained  by  taking  the  vector  dot 
product  of  the  moment  vector  acting  on  the  link  and  the  joint 
axis  vector. 

x,  =  ni-zl  (60) 
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b.    Manipulator  Jacobian   in    the  Force  Domain 

When  an  object  is  displaced  through  some  distance 
5x  by  some  force  F,  work  is  performed.  The  same  idea  holds 

true  for  a  manipulator  arm  as  it  articulates.  Allowing  the 
displacement  to  become  inf initesimally  small  (principle  of 
virtual  work)  and  knowing  that  the  amount  of  work  performed  is 
the  dot  product  of  a  vector  force  or  torque  and  a  vector 
displacement,  the  following  relationship  may  be  written: 

Fbx  =  f6Q  (61) 

or 

FTbx  =  xT,66  <62> 

where  F  is  a  6x1  Cartesian  force-moment  vector  acting  at  the 
end-effector,  8x  is  a  6x1   infinitesimal  Cartesian  displacement 

of  the  end  effector,  r  is  a  6x1   vector  of  joint  torques  and  50 

is  a  6x1  vector  of  joint  displacements.  Using  the  definition 
of  the  Jacobian 

6x  =  JS6  (63) 

substituting  into  Equation  (63)  and  transposing  yields 

X  =  JTF  (64) 

This  result  states  that  the  Jacobian  transpose  maps  the 
cartesian  forces  seen  at  the  end-effector  into  equivalent 
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joint  torques.  The  PUMA  Manipulator   Jacobian   is   developed  in 
Appendix    C. 


B.    IMPLEMENTATION 

1 .   Force  Override  Rate  Controller  Algorithm 

As  seen  from  the  previous  section,  utilization  of  the 
Jacobian  transpose  as  an  operator  on  the  force  matrix  allows 
us  to  compute  the  equivalent  joint  torque  matrix.  If  the 
individual  joint  angles  can  be  read,  then  the  required  change 
in  joint  angles  may  be  computed  from  the  algorithm  in  Figure 
22,  where  q    is  a  generalized  joint  variable  accounting  for 

rotary  as  well  as  prismatic  joints  [9,      d)      and  k     is  an 

arbitrary  constant  to  be  selected  experimentally. 
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Figure  22.   Force  Control  Algorithm 
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V.   CONCLUSIONS 


A.  EXPERIMENTAL  AND  THEORETICAL  RESULTS 

-  Force  override  rate  control  works  as  demonstrated  by  the 
single  degree  of  freedom  hydraulic  actuation  system. 

-  A  force-torque  transducer  may  be  constructed  using  force 
sensing  resistors. 

-  Two  examples  of  transducers  have  been  designed  in  which  a 
specific  pattern  of  FSR's  may  be  used  to  measure  the 
desired  forces  and  torques  applied  to  the  transducer. 

-  The  method  of  checking  the  system  coefficient  matrix  A 
provides  a  design  approach  for  any  order  transducer. 

B.  FURTHER  WORK 

-  Calibrate  the  current  prototype  to  measure  two  force  and 
two  moment  components. 

-  Investigate  the  feasibility  of  sensor  redundancy. 

-  Construct  a  new  reduced  order  prototype  using  a  larger 
cube  so  that  individual  FSR's  do  not  overlap  the  ends  of 
the  cube . 

-  Upon  successful  calibration  and  testing  of  prototype, 
construct  a  second  transducer  to  be  used  as  a  joystick. 

-  Implement  transducers  in  a  controller  algorithm  for 
testing  using  the  PUMA   560. 
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APPENDIX  A 

The  following  table  is  based  on  the  sensor  placement 
pattern  depicted  in  Figure  13  and  the  sensor  equations 
developed  in  Chapter  III. 

TABLE  7.   54  SENSOR  COEFFICIENT  ARRAY 


F  + 

X 

Fx~ 

F  + 

y 

V 

Fz+ 

Fz~ 

M*+ 

"x" 

V 

V 

M2+ 

Mz~ 

S! 

19 

0 

0 

0 

0 

0 

0 

0 

0 

3 

0 

3 

S2 

19 

0 

0 

0 

0 

0 

0 

0 

0 

3 

0 

0 

S3 

19 

0 

0 

0 

0 

0 

0 

0 

0 

3 

3 

0 

S< 

4 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

3 

S5 

4 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

S6 

4 

0 

0 

0 

0 

0 

0 

0 

0 

0 

3 

0 

s7 

0 

11 

0 

0 

0 

0 

0 

0 

3 

0 

0 

3 

Se 

0 

11 

0 

0 

0 

0 

0 

0 

3 

0 

0 

0 

S9 

0 

11 

0 

0 

0 

0 

0 

0 

3 

0 

3 

0 

S10 

0 

19 

0 

0 

0 

0 

0 

0 

3 

0 

0 

3 

Su 

0 

19 

0 

0 

0 

0 

0 

0 

3 

0 

0 

0 

S12 

0 

19 

0 

0 

0 

0 

0 

0 

3 

0 

3 

0 

S!3 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

0 

3 

Sl4 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

S15 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

3 

0 

S16 

11 

0 

0 

0 

0 

0 

0 

0 

0 

3 

0 

3 

S17 

11 

0 

0 

0 

0 

0 

0 

0 

0 

3 

0 

0 

Sie 

11 

0 

0 

0 

0 

0 

0 

0 

0 

3 

3 

0 

S19 

0 

0 

19 

0 

0 

0 

3 

0 

0 

0 

0 

3 

S20 

0 

0 

19 

0 

0 

0 

3 

0 

0 

0 

0 

0 

S21 

0 

0 

19 

0 

0 

0 

3 

0 

0 

0 

3 

0 

s22 

0 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

3 

S23 

0 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

0 

S2< 

0 

0 

4 

0 

0 

0 

0 

0 

0 

0 

3 

0 

S25 

0 

0 

0 

11 

0 

0 

0 

3 

0 

0 

0 

3 

S26 

0 

0 

0 

11 

0 

0 

0 

3 

0 

0 

0 

0 

s27 

0 

0 

0 

11 

0 

0 

0 

3 

0 

0 

3 

0 

26 

0 

0 

0 

19 

0 

0 

0 

3 

0 

0 

0 

3 

S29 

0 

0 

0 

19 

0 

0 

0 

3 

0 

0 

0 

0 

S3C 

0 

0 

0 

19 

0 

0 

0 

3 

0 

0 

3 

0 
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S3! 

0 

0 

0 

4 

0 

0 

0 

0 

0 

0 

0 

3 

S32 

0 

0 

0 

4 

0 

0 

0 

0 

0 

0 

0 

0 

S33 

0 

0 

0 

4 

0 

0 

0 

0 

0 

0 

3 

0 

S3< 

0 

0 

11 

0 

0 

0 

3 

0 

0 

0 

0 

3 

S35 

0 

0 

11 

0 

0 

0 

3 

0 

0 

0 

0 

0 

S36 

0 

0 

11 

0 

0 

0 

3 

0 

0 

0 

3 

0 

S37 

0 

15 

15 

0 

0 

3 

0 

3 

0 

0 

0 

S3B 

0 

15 

0 

0 

0 

0 

0 

3 

0 

0 

0 

S39 

0 

15 

0 

15 

0 

0 

3 

3 

0 

0 

0 

S<o 

0 

0 

15 

0 

0 

3 

0 

0 

0 

0 

0 

s« 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

s<2 

0 

0 

0 

15 

0 

0 

3 

0 

0 

0 

0 

s43 

15 

0 

15 

0 

0 

3 

0 

0 

3 

0 

0 

S44 

15 

0 

0 

0 

0 

0 

0 

0 

3 

0 

0 

S45 

15 

0 

0 

15 

0 

0 

3 

0 

3 

0 

0 

S46 

0 

15 

0 

15 

0 

0 

3 

3 

0 

0 

0 

S47 

0 

15 

0 

0 

0 

0 

0 

3 

0 

0 

0 

S4e 

0 

15 

15 

0 

0 

3 

0 

3 

0 

0 

0 

s49 

0 

0 

0 

15 

0 

0 

3 

0 

0 

0 

0 

S5C 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

s51 

0 

0 

15 

0 

0 

3 

0 

0 

0 

0 

0 

S52 

15 

0 

0 

15 

0 

0 

3 

0 

3 

0 

0 

s53 

15 

0 

0 

0 

0 

0 

0 

0 

3 

0 

0 

S54 

15 

0 

15 

0 

0 

3 

0 

0 

3 

0 

0 

The  full  order  sensor  coefficient  matrix  is  the  54x12 
array  in  Table  7  (rather  than  the  transpose  in  previous 
cases) .  Premultiplying  this  matrix  by  its  transpose  yields  a 
full  rank  12x12  matrix,  thus  verifying  that  all  12  force- 
torque  components  are  determinable.  Having  determined  the 
upper  bound  for  the  number  of  sensors  n,  and  knowing  the 
lower  limit  to  be  12,  individual  sensors  were  eliminated  in 
patterns.  The  procedure  is  outlined  in  Table  8. 
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TABLE  8.   SENSOR  ELIMINATION  STRATEGY 


Action 

Face 

Sensors  Remaining 

Rank 

Eliminate  middle  rows  and  columns 

Delete  S2,S4,  S6,  S8 

X 

50 

12 

Delete  S5 

X 

49 

12 

L-'CICLC  Oil,   O1-3,   Oi4,   uu,   ^17 

X 

44 

12 

Delete  o2o->  ^22'  ^2-v  ^24'  ^26 

Y+ 

39 

12 

Delete  S29,  S3j,  b32,  a33,  S35 

Y 

34 

11 

Add  S32 

Y 

35 

11 

Add  o29,  i>3],  o33,  b35 

Y 

39 

12 

Delete  S38,  o40,  J>4],  o42,  b44 

Z 

34 

12 

Delete  S47,  S49,  S50,  Ssi,  S53 

Z+ 

29 

12 

Eliminate  diagonals 

Delete  S3,  S7 

X+ 

27 

12 

Delete  S10,  S]8 

X 

25 

12 

Delete  S2],  S25 

Y+ 

23 

12 

Delete  S39,  S43 

Z 

21 

12 

Delete  S48,  S52 

Z+ 

19 

12 

Delete  Individual  Sensors 

Delete  S37 

Z 

18 

12 

Delete  S45 

Z 

17 

11 

Add  S45 

Z 

18 

12 

Delete  S46 

Z 

17 

12 

Delete  S9 

X 

16 

11 

AddS9 

X 

17 

12 

Delete  S, 

X 

16 

11 

AddS, 

X+ 

17 

12 

Delete  S28 

Y 

16 

12 

Delete  S30 

Y- 

15 

12 

Delete  S34,  S36 

Y 

13 

12 

Delete  S32 

Y 

12 

12 
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Further  elimination  of  sensors  from  Table  7  results  in  a 
rank  deficient  coefficient  matrix.  The  full  order  sensor 
placement  pattern  is  depicted  in  Figure  14. 

Beginning  with  a  24x8  coefficient  array,  for  which  the 
rank  of  A7  A  was  8,  sensors  were  selectively  eliminated  to 
determine  the  required  placement  pattern  for  the  reduced 
order  sensor. 

TABLE  9.   24  SENSOR  COEFFICIENT  ARRAY 


F  + 

X 

Fx~ 

F  + 

y 

V 

Fz+ 

F2~ 

M2+ 

Mz~ 

sl 

19 

0 

0 

0 

0 

0 

0 

3 

S3 

19 

0 

0 

0 

0 

0 

3 

0 

s7 

0 

11 

0 

0 

0 

0 

0 

3 

S9 

0 

11 

0 

0 

0 

0 

3 

0 

S10 

0 

19 

0 

0 

0 

0 

0 

3 

s12 

0 

19 

0 

0 

0 

0 

3 

0 

s16 

11 

0 

0 

0 

0 

0 

0 

3 

Sxe 

11 

0 

0 

0 

0 

0 

3 

0 

s19 

0 

0 

19 

0 

0 

0 

0 

3 

s21 

0 

0 

19 

0 

0 

0 

3 

0 

S25 

0 

0 

0 

11 

0 

0 

0 

3 

S27 

0 

0 

0 

11 

0 

0 

3 

0 

29 

0 

0 

0 

19 

0 

0 

0 

0 

S3! 

0 

0 

0 

4 

0 

0 

0 

3 

S33 

0 

0 

0 

4 

0 

0 

3 

0 

S35 

0 

0 

11 

0 

0 

0 

0 

0 

S37 

0 

15 

15 

0 

0 

4 

0 

0 

S39 

0 

15 

0 

15 

0 

4 

0 

0 

S«3 

15 

0 

15 

0 

0 

4 

0 

0 

S45 

15 

0 

0 

15 

0 

4 

0 

0 

s« 

0 

15 

0 

15 

4 

0 

0 

0 

S<e 

0 

15 

15 

0 

4 

0 

0 

0 

S52 

15 

0 

0 

15 

4 

0 

0 

0 

S5< 

15 

0 

15 

0 

4 

0 

0 

0 
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TABLE  10.  SENSOR  ELIMINATION  STRATEGY 


Action 

Face 

Sensors  Remaining 

Rank 

Eliminate  diagonals 

Delete  Sv  S7 

X+ 

22 

8 

Delete  S!2,  S]6 

X 

20 

8 

Delete  S21,  S25 

Y+ 

18 

8 

Delete  S39,  S43 

Z 

16 

8 

Delete  S48,  S52 

Z+ 

14 

8 

Eliminate  Individual  Sensors 

Delete  S45 

Z 

13 

8 

Delete  S54 

Z 

12 

8 

Delete  S9 

X 

11 

8 

Delete  S]8 

X 

10 

8 

Delete  S27 

Y^ 

9 

8 

Delete  S„ 

Y 

8 

8 

Again,  further  elimination  of  sensors  resulted  in  a  rank 
deficient  coefficient  matrix.  The  indiviual  sensor  placement 
pattern  for  the  reduced  order  sensor  is  depicted  in  Figure 
15. 
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APPENDIX  B 

Referring  to  Figure  20  and  the  link  and  joint  parameters 
for  the  PUMA  560  given  in  Table  6  the  individual 
transformation  matrices  are  computed  as  follows  from  Equation 
(49)  [Ref.17].  Ca  and  SA  are  short  for  cos9x   and  s±n6x.    Ci;  and 

Sxj   are  cos(0x   +  6D)    and  s±n(6±  +    6J    respectively. 


T? 


C±     0     -Sx    0 

S1  0  Cx  0 
0-100 
0      0       0      1 


tv1  = 


c, 


S2    0    a2  C2 


52       C2      0     (32o2 

0      0     1     d2 
0       0      0       1 


c2 

0 

s2 

a3C3 

s3 

0 

-c2 

a3S3 

0 

1 

0 

0 

0 

0 

0 

1 

T.3   = 


c4 

0 

s* 

0 

£4 

0 

Q 

0 

0 

-1 

0 

d< 

0 

0 

0 

1 
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t:  = 


c5 

0 

S5     0 

s5 

0 

-c5  0 

0 

1 

0      0 

0 

0 

0      1 

TZ   = 


c6 

-S6    0 

0 

s* 

C6     0 

0 

0 

0      1 

de 

0 

0      0 

1 

Multiplying  the  first  and  last  three  transformation  matrices 
together  produces  the  following 


^1^23      _l^l     ^1*^23     ^2^1  ^2     +    ^3^1^23     ~    ^2*^1 

ra 

=  rfra1^ 

- 

^1^-23       ^1       ^1^23     a2*^l^--2     +    ^3*^1  ^23     +    ^2^1 

-523      0        C23               -a2S2  '"  33o23 
0          0          0                              1 

C4C^C6    ■-  S4S6    ~C4C5C6    -   S4S6    C4Sb       Q6C4S5 

r2  ^ 

r43r54:r65  = 

■^4^5^6     +    ^4^6     "'^4^' 5*^6     +    QQ     ^4^5         ^6  ^4*^5 

-55C6                      5556                C5      cf6C5   +  d4 

0                             0                    0               1 

Multiplying  T2  and  T2  gives  the  manipulator  arm  matrix- 


es0 =  r3°r63  -  r.r. 


•^11  ri2  *13  -Px 

r21  r22  r23  Py 

r31  r32  r33  Pz 

0          0          0  1 
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*ii   =   ^[C^iC^C^   -  5456)     -  523S5C6]     -  ^(S^Q    +   C4S6) 


*2i  -  ^[^(c^q  -  S4S6)    -  S23S5C6]    +  q^c^  +  C4S6) 


r3i   '       ^23  l-QCgCg    -  S4S6]     -  C23SbCe 


r12  =  ^[^(C^Q    -  S4S6)    -  s22sbc6]    +  q(s4c5c6  +  C4S6) 


r22   =  ^[-(^(C^Q   +  S4S6)    +  523S5C6]    +  q(-S4C5C6   +  C4S6) 


"^32      =    "^23  '  ^4^5*^6     +    *^4  Q  '      +     ^-23*^5 *^6 


■^13     ::    Hi  '  H23  ^4*^5     +    ^23^5'       "    ^1^4  ^5 


^23     :;    *^l' ^23^4^5     +    "^23  ^5'        '    ^1*^4 *5 


r33     :  ^23^4^5     +     ^23  ^5 


Px  =  q[ck(C23C4S5  +  S23C5)    +  S23d4  +  a3C23  +  a2C2]    -  Sx(d6S4S5  +  d2) 
py  =  Sx[d6(C23C4S5  +  523C5)    +  S22dA  +  a3C23  +  a2C2]    +  C^d^S^  +  d2) 


Pz    ~    U6  \C22C5  ^22^-4^5'      +     ^23*^4     ""    a3"^23      "    a2^2 


63 


APPENDIX  C 

Fu  [Ref .  18]  derives  the  PUMA  560  Manipulator  Jacobian   by 

the  Differential   Translation  and  Rotation  Method.    The  columns 
of  this  6x6  matrix  are  as  follows: 


j±m  = 


j. 


lx 


J. 


ly 


-  L52o  ( C4CcC6   ■■  S.S6)     -   C23o5C6J 


'23 
'23 


■J->^  \  (-a  Lc,ibfi     '•"    ib^  Cg  J      +    ^23     5     6 


'4W5"6 


•^23  ^"4^5  ^23^5 


where 


^y   = 


^ 


[d6(C23C4S5   +    S23C5)    +    d4S23  +    a3C23  +    a2C2]  {S4C5C6  +    C456) 
(d6S4S5   +    d2)  [C23(C4C5C6   -    S4S6)    -    S23S5C6] 
d6(C23C4S5  +   S23C5)    +   d4S23  +   a3C23  +   a2C2]  (-S4C5S6  +    C4C6) 
(d6S4S5   +    d2)  [-C23(C4C5S6  +    S4C6)    +    S23S5S6] 
d6(C23C4S5   +    S23C5)    +    d4S23  +    a3C23  +    a2C2]  (S4S5) 
(d6S4S5   +    d2)  (C23C4S5   +    S23C5) 


where 


Jo  (6) 


'2x 


J- 


2y 

^2z 

o4  C5  C6    +    C4  o  6 
-iS4C5o6    +   C456 

s4s5 


J2x  =    (dbS3C,   +    d6C3C4Sb   +    d4S3  +    a3C3  +    a2)  (S5C 
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J 


2y 


-{-dtC3C5   +    d6S3C4S5   -    d4C3  +    a3S3)  {C4CiC6   -    S4Se 
--   -  {d6S3C5   +    d6C3C4S5   +    d4S3  +    a3C3  +    a2)  (55S6) 


+  (-dtC3C5   +    d6S3C4S5   -    d4C3  +    a3S3)  {C4S5) 
J22   -   -(d^Q   +    d6C3C4S5   +    d4S3  +    a3C3  +    a2)  C5 
-(-d6C3C5   +    d6S3C4S5   -    d4C3  +    a3S3)  (C4S5) 


oue; 


(a3  +  d6C4S5)  (55C6)    +    (d4  +  d6C5)  (C4CSC6  -  S4S6) 

-(a3   +  d6C4Ss)  (S5S6)        (d4   +  d6C5)  (C4C5C6   +  S4S6] 

-(a,  +  d6C4S=)a  +    (d4  +  d6C5)C4S, 


6^5'  ^4-^5 


o  4  C5  C6    +    C4  o6 
-S4CbS6    +   C4S6 

S4S5 


J.(0)    = 


^s5c6 


-  q  n 


Jc(e)  = 


65 


^(6)    = 
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